#!/usr/bin/env python3

import rospy
import std_msgs.msg
from geometry_msgs.msg import Twist
from sensor_msgs.msg import RegionOfInterest as ROI
import numpy as np

w = 640
h = 480
pid_w = [0.21, 0, 0] # pid parameters of yaw channel
pid_h = [0.21, 0, 0] # pid parameters of up channel
pid_f = [0.8, 0, 0] # pid parameters of forward channel
i=0
list_forward_speed=[]
list_up_speed=[]
list_yaw_speed=[]
def callback(msg):
    global i
    tld_roi = msg
    cx = tld_roi.x_offset + tld_roi.width // 2
    cy = tld_roi.y_offset + tld_roi.height // 2
    area = tld_roi.width * tld_roi.width
    yaw_speed, up_speed, forward_speed = pidtrack(cx, cy, area, w, h, pid_w, pid_h, pid_f)
    list_forward_speed.append(forward_speed)
    list_up_speed.append(up_speed)
    list_yaw_speed.append(yaw_speed)
    i=i+1
    if i>=3:
        fliter_forward_speed = (list_forward_speed[i-1]+list_forward_speed[i-2]+list_forward_speed[i-3])/3
        fliter_up_speed = (list_up_speed[i-1]+list_up_speed[i-2]+list_up_speed[i-3])/3
        fliter_yaw_speed = (list_yaw_speed[i-1]+list_yaw_speed[i-2]+list_yaw_speed[i-3])/3
        rc ="This is " + str(i) +  "forward: " + str(fliter_forward_speed) + " up: " + str(fliter_up_speed) + "  yaw: " + str(fliter_yaw_speed)
    
        speed = Twist()
        if fliter_yaw_speed != 0 or fliter_up_speed != 0 or fliter_forward_speed != 0:
            speed.linear.x = fliter_forward_speed
            speed.linear.y = 0.0
            speed.linear.z = fliter_up_speed
            speed.angular.x = 0.0
            speed.angular.y = 0.0
            speed.angular.z = fliter_yaw_speed
            rospy.loginfo(rc)
            pub.publish(speed)
        else:
            if not zero_twist_published:
                pub.publish(speed)
                rospy.loginfo("no object detected")

def pidtrack(cx, cy, area, w, h, pid_w, pid_h, pid_f):
    ## PID
    # yaw channel
    error_w = cx - w // 2
    speed_w = pid_w[0] * error_w
    speed_w = int(np.clip(speed_w, -100, 100)) / 100.0

    # up channel
    error_h = cy- h // 2
    speed_h = pid_h[0] * error_h
    speed_h = int(np.clip(speed_h, -100, 100)) / 100.0

    # forward channel
    error_f = np.sqrt(area) - 140
    speed_f = pid_f[0] * error_f
    speed_f = int(np.clip(speed_f, -100, 100)) / 100.0

    if cx != 0:
        yaw_speed = -speed_w
    else:
        yaw_speed = 0
    if cy != 0:
        up_speed = -speed_h
    else:
        up_speed = 0
    if area != 0:
        forward_speed = -speed_f
    else:
        forward_speed = 0

    return yaw_speed, up_speed, forward_speed

if __name__ == '__main__':
    #/mavros/setpoint_velocity/cmd_vel_unstamped
    rospy.init_node('pid', anonymous=True)
    sub = rospy.Subscriber("roi", ROI, callback)
    pub = rospy.Publisher('three_fliter', Twist, queue_size=1)
    rospy.spin()
